#include "encoder.h"

Encoder::Encoder() {}

void Encoder::init() {
    // Enable GPIO and SPI3 clocks
    __HAL_RCC_GPIOA_CLK_ENABLE();
    __HAL_RCC_GPIOC_CLK_ENABLE();
    __HAL_RCC_SPI3_CLK_ENABLE();
    
    // Configure GPIO pins
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    
    // PC10 - SCK, PC11 - MISO
    GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.Alternate = GPIO_AF6_SPI3;
    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
    
    // PA4 - NSS
    GPIO_InitStruct.Pin = GPIO_PIN_4;
    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
    
    // Configure SPI3
    hspi3.Instance = SPI3;
    hspi3.Init.Mode = SPI_MODE_MASTER;
    hspi3.Init.Direction = SPI_DIRECTION_2LINES_RXONLY;
    hspi3.Init.DataSize = SPI_DATASIZE_8BIT;
    hspi3.Init.CLKPolarity = SPI_POLARITY_LOW; // MT6701 transmit data on rising edge
    hspi3.Init.CLKPhase = SPI_PHASE_2EDGE;  // STM32 Sample data on falling edge
    hspi3.Init.NSS = SPI_NSS_SOFT;
    hspi3.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32;
    hspi3.Init.FirstBit = SPI_FIRSTBIT_MSB;
    hspi3.Init.TIMode = SPI_TIMODE_DISABLE;
    hspi3.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
    
    HAL_SPI_Init(&hspi3);
}

uint32_t Encoder::readRawData() {
    uint8_t data[3] = {0};
    
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);  // CS low
    HAL_SPI_Receive(&hspi3, data, 3, HAL_MAX_DELAY);
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);    // CS high
    
    // Combine 3 bytes into 24-bit value
    return (uint32_t)((data[0] << 16) | (data[1] << 8) | data[2]);
}

float Encoder::readAngle() {
    uint32_t rawData = readRawData();
    uint16_t angleData = (rawData >> 10) & 0x3FFF;
    return angleData * ANGLE_SCALE;  // Now returns radians directly
}

float Encoder::normalizeAngle(float angle) {
    while(angle >= 2.0f * PI) angle -= 2.0f * PI;
    while(angle < 0.0f) angle += 2.0f * PI;
    return angle;
}

void Encoder::updateAngles() {
    theta_mech = readAngle();  // Now in radians
    theta_elec = normalizeAngle(theta_mech * POLE_PAIRS);  // Normalize in radians
}

float Encoder::getElectricalAngle() const {
    return theta_elec;
}
